#!/usr/bin/env python
# RSBand Local Planner configuration

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

# common parameters
rear_steering_mode_enum = gen.enum(
    [
        gen.const("None", int_t, 0, "No rear steering"),
        gen.const("Counter", int_t, 1, "Rear steering of same magnitude, but opposite direction from front steering"),
        gen.const("Crab", int_t, 2, "Crab rear steering in case of lateral deviation"),
        gen.const("Hybrid", int_t, 3, "A hybrid steering mode, combining crab and counter steering")
    ], "An enum for setting rear steering mode")
gen.add("rear_steering_mode", int_t, 0, "The rear steering mode which is setted via an enum", 0, 0, 3, edit_method=rear_steering_mode_enum)
gen.add("stop", bool_t, 0, "When stop is true publish zero velocity commands", False)
gen.add("max_speed", double_t, 0, "The maximum speed of the robot", 0.2, 0.1, 2.0)
gen.add("max_steering_angle", double_t, 0, "The maximum steering angle of the virtual middle wheel", 0.4, 0.0, 1.57)
gen.add("wheelbase", double_t, 0, "The wheelbase of the robot", 0.5, 0.1, 2.0)
gen.add("xy_goal_tolerance", double_t, 0, "The maximum distance to the goal, that the goal is perceived as reached, regarding distance", 0.1, 0.0, 5.0)
gen.add("yaw_goal_tolerance", double_t, 0, "The maximum difference in orientation, that the goal pose is perceived as reached, regarding orientation", 0.1, 0.0, 3.1415)
gen.add("update_sub_goal_dist_threshold", double_t, 0, "When the distance between the robot and the current sub goal is less than this value, select new sub goal", 0.4, 0.0, 5.0)
gen.add("emergency_planning", bool_t, 0, "Determines whether emergency planning will be used in case of planning fails", True)

# reeds shepp planner parameters
eband_to_rs_strategy = gen.enum(
    [
        gen.const("start_to_end", int_t, 0, "Strategy that connects eband start and end waypoints using a reeds shepp path"),
        gen.const("start_to_next", int_t, 1, "Strategy that connects eband start and next waypoints using a reeds shepp path"),
        gen.const("point_to_point", int_t, 2, "Strategy that connects eband waypoints using reeds shepp paths"),
        gen.const("skip_failures", int_t, 3, "Strategy that connects eband waypoints using reeds shepp paths, but skips waypoints that cause failure"),
        gen.const("start_to_receding_end", int_t, 4, "Strategy that starts as the start_to_end strategy, and in case of failure, uses, previous to end, eband waypoint")
    ], "An enum for setting eband_to_rs_strategy")
state_checking_mode = gen.enum(
    [
        gen.const("center", int_t, 0, "State validity checking by calculating the cost of center of robot footprint"),
        gen.const("footprint", int_t, 1, "State validity checking by calculating the max cost of the footprint cells"),
    ], "An enum for setting the state checking mode to center or footprint cost checking")
gen.add("eband_to_rs_strategy", int_t, 0, "The strategy to convert an eband to a reeds shepp paths", 1, 0, 4, edit_method=eband_to_rs_strategy)
gen.add("state_checking_mode", int_t, 0, "Select state checking mode between center and footprint cost checking", 0, 0, 1, edit_method=state_checking_mode)
gen.add("max_planning_duration", double_t, 0, "The maximum allowed planning duration for a reeds shepp plan", 0.1, 0.01, 1.0)
gen.add("interpolation_num_poses", int_t, 0, "Number of interpolated poses of the reeds shepp path", 20, 5, 1000)
gen.add("robot_state_valid", bool_t, 0, "Determines whether robot state is considered always valid, even close to obstacles", False)
gen.add("valid_state_max_cost", int_t, 0, "Max footprint cost of a valid state in a reeds shepp plan", 252, 0, 254)
gen.add("allow_unknown", bool_t, 0, "Determines perception of unknown costmap cells as traversable or not", True)
gen.add("display_planner_output", bool_t, 0, "If true, the node displays planning output from ompl reeds shepp state space", False)

# car like fuzzy path tracking controller parameters
gen.add("goal_dist_threshold", double_t, 0, "The threshold that determines closeness to the goal", 0.2, 0.0, 2.0)
gen.add("lateral_deviation_tolerance", double_t, 0, "Denotes the maximum acceptable lateral deviation from the plan", 0.1, 0.0, 1.0)
gen.add("display_controller_io", bool_t, 0, "If true, display controller input and output information", False)


exit(gen.generate("rsband_local_planner", "rsband_local_planner", "RSBandPlanner"))
